#include <mc_core/msg_convert/msg_convert.h>

namespace msg_convert{

    /***************************************************
     * to string
     ****************************************************/
    // geometry_msgs::Pose -> string
    string toString(geometry_msgs::Pose pose){
        std::stringstream ss;
        ss << "(" << pose.position.x << ", " << pose.position.y << ", " << pose.position.z 
            << " | " << tf2::getYaw(pose.orientation) << ")";
        return ss.str();        
    }

    // geometry_msgs::Twist -> string
    string toString(geometry_msgs::Twist vel){
        std::stringstream ss;
        ss << "(" << vel.linear.x << ", " << vel.linear.y << ", " << vel.angular.z 
            << ")";
        return ss.str();        
    }


    /***************************************************
     * to Point2f
     ****************************************************/
    mc::Point2f toPoint2f(geometry_msgs::Pose pose){
        return mc::Point2f(pose.position.x, pose.position.y);
    }



    /***************************************************
     * to PoseStamped
     ****************************************************/
    geometry_msgs::PoseStamped toPoseStamped(geometry_msgs::Pose pose, string frame_id){
        geometry_msgs::PoseStamped pose_stamped;
        pose_stamped.header.frame_id = frame_id;
        pose_stamped.header.stamp = ros::Time::now();
        pose_stamped.pose = pose;
        return pose_stamped;
    }


    /***************************************************
     * to Path
     ****************************************************/
    nav_msgs::Path toPath(vector<mc::Point2f>& points, string frame_id){
        nav_msgs::Path path_msg;
        path_msg.header.frame_id = frame_id;
        path_msg.header.stamp = ros::Time::now();
        for(auto& p : points){
            geometry_msgs::PoseStamped pose;
            pose.header.frame_id = frame_id;
            pose.pose.position.x = p.x;
            pose.pose.position.y = p.y;
            pose.pose.orientation.z = 1.0;
            path_msg.poses.push_back(pose);
        }
        return path_msg;
    }


    /***************************************************
     * Quaternion 转换
     ****************************************************/
    // geometry_msgs::Quaternion -> tf2::Quaternion
    tf2::Quaternion toTFQuaternion(geometry_msgs::Quaternion q){
        tf2::Quaternion q_tf;
        tf2::fromMsg(q, q_tf);
        return q_tf;
    }

    // tf2::Quaternion -> geometry_msgs::Quaternion
    geometry_msgs::Quaternion toGeometryQuaternion(tf2::Quaternion q){
        return tf2::toMsg(q);
    }

    /***************************************************
     * 四元数 与 欧拉角、偏航角 转换
     ****************************************************/
    //euler -> tf2::Quaternion
    tf2::Quaternion toTFQuaternion(double roll, double pitch, double yaw){
        tf2::Quaternion q_tf;
        q_tf.setRPY(roll, pitch, yaw);
        return q_tf;
    }

   // euler -> geometry_msgs::Quaternion
    geometry_msgs::Quaternion toGeometryQuaternion(double roll, double pitch, double yaw){
        return toGeometryQuaternion(toTFQuaternion(roll, pitch, yaw));
    }

    // tf2::Quaternion -> euler
    void toEuler(tf2::Quaternion q_tf, double& roll, double& pitch, double& yaw){
        tf2::Matrix3x3 mat(q_tf);
        mat.getRPY(roll, pitch, yaw);
    }

    // geometry_msgs::Quaternion -> euler
    void toEuler(geometry_msgs::Quaternion q, double& roll, double& pitch, double& yaw){
        tf2::Quaternion q_tf = toTFQuaternion(q);
        toEuler(q_tf, roll, pitch, yaw);
    }

    // tf2::Quaternion -> yaw
    double toYaw(tf2::Quaternion q_tf){
        // return tf2::getYaw(q);
        double roll, pitch, yaw;
        toEuler(q_tf, roll, pitch, yaw);
        return yaw;
    }

    // geometry_msgs::Quaternion -> yaw
    double toYaw(geometry_msgs::Quaternion q){
        // return tf2::getYaw(q);
        return toYaw(toTFQuaternion(q));
    }



};  // namespace